'''
This module includes utility routines that are commonly used by other programs.

Here is a list of the utility routines:

rotation_matrix: The matrix for rotating a 3D point around an axis 
    by an angle.

vec_len: magnitude of a vector.

area_triangle: 
area_triangle_coordinates: calculate the area of a triangle by three edges
	(positive numbers a, b, c for which a+b>c, b+c>a, c+a>b suffice) or
	by three coordinates (three-element or two-element array; allowing
	overlapping).

Created on January 28, 2012
@author: haibo
'''
import numpy as np

def rotation_matrix(u, theta):
    """ returns transformation matrix of a rotation around u by theta.
    
    u is the rotation axis (numpy.ndarray).
    theta is the rotation angle, in radius degree.
    Let u be normalized, then the rotation matrix is
    (from http://en.wikipedia.org/wiki/Rotation_matrix)
    R11 = cos(theta) + ux**2 * (1-cos(theta)), 
    R12 = ux * uy * (1-cos(theta)) - uz * sin(theta)
    R13 = ux * uz * (1-cos(theta)) + uy * sin(theta)
    R21 = uy * ux * (1-cos(theta)) + uz * sin(theta)
    R22 = cos(theta) + uy**2 * (1-cos(theta))
    R23 = uy * uz * (1-cos(theta)) - ux * sin(theta)
    R31 = uz * ux * (1-cos(theta)) - uy * sin(theta)
    R32 = uz * uy * (1-cos(theta)) + ux * sin(theta)
    R33 = cos(theta) + uz**2 * (1-cos(theta))
    """
    
    # normalize the vector u
    u_len = np.sqrt(u[0]**2 + u[1]**2 + u[2]**2)
    um = u / u_len

    # make it easier to write
    c = np.cos(theta)
    s = np.sin(theta)
    c1 = 1.0 - c
    ux, uy, uz = um
    
    R = np.matrix([
        [c + ux**2 * c1, ux*uy * c1 - uz * s, ux*uz * c1 + uy * s],
        [uy*ux * c1 + uz * s, c + uy**2 * c1, uy*uz * c1 - ux * s],
        [uz*ux * c1 - uy * s, uz*uy * c1 + ux * s, c + uz**2 * c1] ])
    return R



def vec_len(vec):
    """ computes the magnitude of a vector.
    
    This function trivially invokes numpy.sqrt(numpy.inner(vec, vec)).
    """
    return np.sqrt(np.inner(vec, vec))


def area_triangle(a, b, c):
    """ returns the area of a triangle given the three edge lengths."""
    
    s = (a + b + c ) * 0.5
    area = np.sqrt(s * (s-a) * (s-b) * (s-c))
    if np.isnan(area) :
        print "I found a nan in calculating area. a, b, c are", a, b, c
        print "This usually means an edge length is very close to zero or negative."
        print "I will return zero for this situation."
        return 0.0
    return area


def area_triangle_coordinates(c1, c2, c3):
    """ returns the area of a triangle given the three coordinates."""
    a = vec_len(np.array(c2) - np.array(c3))
    b = vec_len(np.array(c3) - np.array(c1))
    c = vec_len(np.array(c1) - np.array(c2))
    return area_triangle(a, b, c)

